#ifndef MODEL_INVERSE_KINEMATICS_H
#define MODEL_INVERSE_KINEMATICS_H

#include "model/model_impl/forward_kinematics.hpp"
#include "model/model_impl/robot_model.hpp"
#include "model/model_data_structure.hpp"
#include "utilities/math.hpp"

namespace ZORAL
{
    class AnalyticalIKOfSRS
    {
    private:
        double d_bs_{};
        double d_se_{};
        double d_ew_{};
        double d_wf_{};

        Position p_02_;
        Position p_67_;

        ForwardKinematics *fk_;

    public:
        explicit AnalyticalIKOfSRS(ForwardKinematics *fk)
        {
            fk_ = fk;
        }

        int setGeometryParam(const double &d_bs, const double &d_se, const double &d_ew, const double &d_wf);
        double computeArmAngle(const Vector7d &joint_angles);
        std::vector<HomoTrans> computeAnalyticalFK(const Vector7d &joint_angles);
        Vector7d computeUniqueAnalyticalIK(const HomoTrans &transform_matrix, const double &arm_angle,
                                           const int &global_config);
        int getGlobalConfig(const Vector7d &q);
        Vector7d computeVirtualAngles(const HomoTrans &transform_matrix, const int GC4);
    };


    class InverseKinematics
    {
    private:
        ForwardKinematics *fk_;
        AnalyticalIKOfSRS *ik_analytical_;

    public:
        explicit InverseKinematics(ForwardKinematics *fk)
        {
            fk_ = fk;
            ik_analytical_ = new AnalyticalIKOfSRS(fk_);

            if (fk_->getRobotModel()->getRobotName() == "zoram_one_alpha")
            {
                ik_analytical_->setGeometryParam(0.155, 0.29, 0.26, 0.09);
            }
        }

        AnalyticalIKOfSRS *getIkAnalytical()
        {
            return ik_analytical_;
        }

        int calInverseKinematicsAnalytical(const HomoTrans &T, const double &arm_angle, const int &global_config,
                                           VectorXd &q);
        int calInverseKinematicsAnaSample(const HomoTrans &T, const VectorXd &q_last, VectorXd &q);

    };
} // namespace ZORAL
#endif